#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import os

class ImageSubscriber:
    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/ros2_image', Image, self.callback)
        self.save_dir = './out/save/images'
        if not os.path.exists(self.save_dir):
            os.makedirs(self.save_dir)

    def callback(self, data):
        try:
            # 转换ROS图像为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            # 生成唯一文件名
            timestamp = rospy.Time.now().to_nsec()
            filename = f"{self.save_dir}/image_{timestamp}.jpg"
            # 保存图像
            cv2.imwrite(filename, cv_image)
            rospy.loginfo(f'Saved image to {filename}')
        except Exception as e:
            rospy.logerr(f'Error processing image: {e}')

if __name__ == '__main__':
    rospy.init_node('image_subscriber_ros1')
    subscriber = ImageSubscriber()
    rospy.spin()